/*
 * Type_conversion.c
 *
 *  Created on: Aug 4, 2024
 *      Author: 27578
 */
#include "Type_conversion.h"
#include "FreeRTOS.h"
#include "queue.h"
#include "cmsis_os2.h"

void nrf24l01_TYPE_CONVERSION_tx(uint8_t *tmp_buf,float *angle,short *angular_velocity,short *UAV_pwm)
{
	short angle_t[3];
	angle_t[0]=(short)(angle[0]*100);
	angle_t[1]=(short)(angle[1]*100);
	angle_t[2]=(short)(angle[2]*100);
	for (uint8_t i = 0; i < 3; ++i)
	{
		for (uint8_t j = 0; j < 2; ++j)
		{
			tmp_buf[i * 2 + j] = ((uint8_t *)&angle_t[i])[j];
		}
	}

	for (uint8_t i = 0; i < 3; ++i)
	{
		for (uint8_t j = 0; j < 2; ++j)
		{
			tmp_buf[i * 2 + j+6] = ((uint8_t *)&angular_velocity[i])[j];
		}
	}
	for (uint8_t i = 0; i < 4; ++i)
	{
		for (uint8_t j = 0; j < 2; ++j)
		{
			tmp_buf[i * 2 + j+12] = ((uint8_t *)&UAV_pwm[i])[j];
		}
	}

}

void nrf24l01_TYPE_CONVERSION_rx(uint8_t *tmp_buf,uint8_t *Unlock,short *throttle,float *angle,short *pid)
{
	short angle_t[3];
	*Unlock = tmp_buf[0];
	for (uint8_t i = 0; i < 1; ++i)
	{
		for (uint8_t j = 0; j < 2; ++j)
		{
			((uint8_t *)&throttle[i])[j] = tmp_buf[i * 2 + j+1];
		}
	}

	for (uint8_t i = 0; i < 3; ++i)
	{
		for (uint8_t j = 0; j < 2; ++j)
		{
			((uint8_t *)&angle_t[i])[j] = tmp_buf[i * 2 + j + 3];
		}
	}
	for (uint8_t i = 0; i < 5; ++i)
	{
		for (uint8_t j = 0; j < 2; ++j)
		{
			((uint8_t *)&pid[i])[j] = tmp_buf[i * 2 + j + 9];
		}
	}
	angle[0]=(float)angle_t[0];
	angle[1]=(float)angle_t[1];
	angle[2]=(float)angle_t[2];
}
